#!/usr/bin/env python
# coding=utf-8


import rospy  # 引用ROS的Python接口功能包
import cv2
import cv_bridge  # 引用opencv功能包。cv_bridge是ROS图像消息和OpenCV图像之间转换的功能包
import numpy as np  # 引用数组功能包
from sensor_msgs.msg import Image  # 引用ROS内的图片消息格式
from geometry_msgs.msg import Twist  # 引用ROS的速度消息格式
import math
import numpy as np
import message_filters  # 时间同步使用


# 动态调节窗口在调试完成得到合适的阈值参数后应该注释掉，以节省系统资源

# 定义一个图片转换的类，功能为：订阅ROS图片消息并转换为OpenCV格式处理，处理完成再转换回ROS图片消息后发布
class Line_Follow:
	def __init__(self):  # 类成员初始化函数
		self.bridge = cv_bridge.CvBridge()  # 初始化图片转换功能，cv_bridge.CvBridge()
		self.image_sub_1 = message_filters.Subscriber("/ShowImage_0", Image, queue_size=1)  # 初始化订阅者,rospy.Publisher()功能是创建订阅者类并输出
		self.image_sub_2 = message_filters.Subscriber("/ShowImage_1", Image, queue_size=1)
		print("odomsub")
		self.odom_sub = message_filters.Subscriber("/odom", Odometry, queue_size=1)
		print("cmd_vel_pub")
		# 初始化发布者,发表话题名为“cmd_vel”,该名与其它节点订阅的控制命令话题要求一致，消息格式为Twist也其它节点订阅的控制命令话题消息格式要求一致
		self.cmd_vel_pub = rospy.Publisher("cmd_vel", Twist, queue_size=1)
		self.twist = Twist()  # 创建速度控制命令变量
		print("ts")
		self.ts = message_filters.ApproximateTimeSynchronizer([self.image_sub_1, self.image_sub_2], 1, 1000, True)
		print("ts callback")
		self.ts.registerCallback(self.callback)

	def callback(self, data1, data2):  # 订阅者接受到消息后的回调函数
		rospy.loginfo("call back")
		# 相机1
		cv_image = self.bridge.imgmsg_to_cv2(data1, "bgr8")  # ROS图片消息转换为OpenCV图片格式
		(rows, cols, channels) = cv_image.shape  # 获得转换后图片的宽度、高度和图片通道数

		blur = cv2.blur(cv_image, ksize=(3, 3))
		gray = cv2.cvtColor(blur, cv2.COLOR_BGR2GRAY)
		ret, thresh0 = cv2.threshold(
		gray, 130, 255, cv2.THRESH_BINARY)  # TODO: 调参
		thresh = thresh0
		edge = cv2.Canny(thresh, 20, 100, 3)  # TODO:调参
		ret, edge = cv2.threshold(edge, 170, 255, cv2.THRESH_BINARY)
		cv2.waitKey(1)
		lines = cv2.HoughLines(edge, 1, math.pi / 180, 50)
		rho, theta1 = lines[0][0]

		a = np.cos(theta1)
		b = np.sin(theta1)
		x0 = a*rho
		y0 = b*rho
		x1 = int(x0 + 200*(-b))
		y1 = int(y0 + 200*(a))
		x2 = int(x0 - 200*(-b))
		y2 = int(y0 - 200*(a))
		cv2.line(cv_image, (x1, y1), (x2, y2), (0, 0, 255), 10)
		cv2.imshow("thresh1", cv_image)

		theta1 = theta1/math.pi*180
		if theta1 > 90:
			theta1 = theta1 - 180
		print("theta1", theta1)


	# 相机2
		cv_image = self.bridge.imgmsg_to_cv2(data2, "bgr8")  # ROS图片消息转换为OpenCV图片格式
		(rows, cols, channels) = cv_image.shape  # 获得转换后图片的宽度、高度和图片通道数
        
		blur = cv2.blur(cv_image, ksize=(3, 3))
		gray = cv2.cvtColor(blur, cv2.COLOR_BGR2GRAY)
		ret, thresh0 = cv2.threshold(gray, 130, 255, cv2.THRESH_BINARY)  # TODO: 调参
		thresh = thresh0

		edge = cv2.Canny(thresh, 20, 100, 3)  # TODO:调参
		ret, edge = cv2.threshold(edge, 170, 255, cv2.THRESH_BINARY)
		cv2.waitKey(1)
		lines = cv2.HoughLines(edge, 1, math.pi / 180, 50)
		rho, theta2 = lines[0][0]
		a = np.cos(theta2)
		b = np.sin(theta2)
		x0 = a*rho
		y0 = b*rho
		x1 = int(x0 + 200*(-b))
		y1 = int(y0 + 200*(a))
		x2 = int(x0 - 200*(-b))
		y2 = int(y0 - 200*(a))
		cv2.line(cv_image, (x1, y1), (x2, y2), (0, 0, 255), 10)
                
		cv2.imshow("thresh2", cv_image)

		theta2 = theta2/math.pi*180
		if theta2 > 90:
			theta2 = theta2 - 180
		print("theta2", theta2)

		self.twist.linear.x = -0.01*theta1
		self.twist.linear.y = 0.01*theta2
		self.twist.angular.z = 0

                # 发布控制命令
		self.cmd_vel_pub.publish(self.twist)
		cv2.waitKey(1)

if __name__ == '__main__':  # 这段判断的作用是，如果本py文件是直接运行的则判断通过执行if内的内容，如果是import到其他的py文件中被调用(模块重用)则判断不通过
	rospy.init_node("OpenCVLineFollow")  # 创建节点
	rospy.loginfo("OpenCVLineFollow node started")  # 打印ROS消息说明节点已开始运行
	Line_Follow()  # 直接运行image_converter()函数创建类，该类在运行期间会一直存在。因为该类没有需要调用的函数，所以使用赋值的形式：a=image_converter()
	rospy.spin()  # 相当于while(1),当订阅者接收到新消息时调用回调函数
